/**
 * @file pid_control.c
 * @brief PID控制及相关滤波功能实现
 * @author 废话文学创始人
 * @version 1.0
 * @date 2024-07-14
 */

#include <pid_control.h>
#include "math.h"

// PID参数容错范围
#define KP_Fault_Tolerance_Range 0.003
#define KI_Fault_Tolerance_Range 0.003
#define KD_Fault_Tolerance_Range 0.003

#define Error_Estimate 500
#define Error_Measure 30

Kalman filter_rp1;
Kalman filter_rp2;
Kalman filter_rp3;
Kalman filter_rp4;
PID pid;

/**
 * @brief 电机PWM输出回调函数
 * @param output PID输出值
 */
static void pidcontrol(float output)
{
    Motor_set_PWM((int)output);
}

/**
 * @brief PID积分分档函数
 * @param error 当前误差
 * @return 分档系数
 */
static float pidGearshiftIntegral(float error)
{
    return 1;
}

/**
 * @brief PID控制系统初始化
 * @note 包括电机、PID、编码器、RP及卡尔曼滤波器初始化
 */
void pid_contorl_init(void)
{
    // Motor init
    Motor_init();
    // PID init
    PID_Init(&pid, 0.5, 0.2, 0, 5, 100, 0.6, 3, pidcontrol, pidGearshiftIntegral);
    // Encoder init
    Encoder_init();
    // RP init
    Kalman_Error_Set(&filter_rp1, Error_Estimate, Error_Measure);
    Kalman_Error_Set(&filter_rp2, Error_Estimate, Error_Measure);
    Kalman_Error_Set(&filter_rp3, Error_Estimate, Error_Measure);
    Kalman_Error_Set(&filter_rp4, Error_Estimate, Error_Measure);
    RP_init();
}

/**
 * @brief RP采集并滤波运行
 * @note 读取RP各通道并进行卡尔曼滤波
 */
void RP_Read_And_filter_RUN(void)
{
    RP_Read_block();
    Kalman_calculate(&filter_rp1, (float)RP_Get_value(1));
    Kalman_calculate(&filter_rp2, (float)RP_Get_value(2));
    Kalman_calculate(&filter_rp3, (float)RP_Get_value(3));
    Kalman_calculate(&filter_rp4, (float)RP_Get_value(4));
}

/**
 * @brief PID控制主流程
 * @note 根据滤波后的RP值设置PID参数和目标，并进行容错处理
 */
void pid_control(void)
{
    // 定速控制
    PID_Parameter_Set(&pid,
                      (float)(filter_rp1.value.estimate.current / 4095.0 * 0.5),
                      (float)(filter_rp2.value.estimate.current / 4095.0 * 0.5),
                      (float)(filter_rp3.value.estimate.current / 4095.0 * 0.5));
    PID_Target_Set(&pid,
                   (float)(filter_rp4.value.estimate.current / 4095.0 * 400 - 200));

    // PID参数容错处理
    if (fabs((double)(filter_rp1.value.estimate.current)) <= KP_Fault_Tolerance_Range)
    {
        pid.parameter.kp = 0;
    }
    if (fabs((double)(filter_rp2.value.estimate.current)) <= KI_Fault_Tolerance_Range)
    {
        pid.parameter.ki = 0;
    }
    if (fabs((double)(filter_rp3.value.estimate.current)) <= KD_Fault_Tolerance_Range)
    {
        pid.parameter.kd = 0;
    }
}
